#include "common.h"

#include "arm/arm.h"

static const u8 num_font[] = { 0b00000000, 0b00011000, 0b00100100, 0b00101100, 0b00110100,
0b00100100, 0b00011000, 0b00000000, 0b00000000, 0b00011000, 0b00101000, 0b00001000, 0b00001000,
0b00001000, 0b00111100, 0b00000000, 0b00000000, 0b00011000, 0b00100100, 0b00000100, 0b00001000,
0b00010000, 0b00111100, 0b00000000, 0b00000000, 0b00111000, 0b00000100, 0b00011000, 0b00000100,
0b00000100, 0b00111000, 0b00000000, 0b00000000, 0b00100100, 0b00100100, 0b00111100, 0b00000100,
0b00000100, 0b00000100, 0b00000000, 0b00000000, 0b00111100, 0b00100000, 0b00111000, 0b00000100,
0b00000100, 0b00111000, 0b00000000, 0b00000000, 0b00011100, 0b00100000, 0b00111000, 0b00100100,
0b00100100, 0b00011000, 0b00000000, 0b00000000, 0b00111100, 0b00000100, 0b00000100, 0b00001000,
0b00010000, 0b00010000, 0b00000000, 0b00000000, 0b00011000, 0b00100100, 0b00011000, 0b00100100,
0b00100100, 0b00011000, 0b00000000, 0b00000000, 0b00011000, 0b00100100, 0b00011100, 0b00000100,
0b00000100, 0b00111000, 0b00000000, 0b00000000, 0b00011000, 0b00100100, 0b00111100, 0b00100100,
0b00100100, 0b00100100, 0b00000000, 0b00000000, 0b00111000, 0b00100100, 0b00111000, 0b00100100,
0b00100100, 0b00111000, 0b00000000, 0b00000000, 0b00011100, 0b00100000, 0b00100000, 0b00100000,
0b00100000, 0b00011100, 0b00000000, 0b00000000, 0b00110000, 0b00101000, 0b00100100, 0b00100100,
0b00101000, 0b00110000, 0b00000000, 0b00000000, 0b00111100, 0b00100000, 0b00111100, 0b00100000,
0b00100000, 0b00111100, 0b00000000, 0b00000000, 0b00111100, 0b00100000, 0b00111100, 0b00100000,
0b00100000, 0b00100000, 0b00000000};

#define PX_TO_ADDR(x, y)	(((239 - (y)) + (240 * (x))) * 3)
void draw_char(u8 *fb, int c, int x, int y)
{
	for (int _y = 0; _y < 8; _y++) {
		for (int _x = 0; _x < 8; _x++) {
			u8 *fbpos = fb + PX_TO_ADDR(x + _x, y + _y);

			u8 mask = (num_font[(c * 8) + _y] >> (8 - _x)) & 1;
			u8 col = mask ? 0xff : 0;

			*(fbpos++) = col;
			*(fbpos++) = col;
			*(fbpos++) = col;
		}
	}
}
void draw_hex(u8 *fb, u32 num, int x, int y)
{
	x += 7*8;
	for (int i = 0; i < 8; i++) {
		draw_char(fb, num & 0xf, x, y);
		num >>= 4;
		x -= 8;
	}
}
static void draw_regs(u32 *regs)
{
	u8 *fb = (u8*)(0x18119400);
	// hardcoded bottom screen

	for (int i = 0; i < 16; i++)
		draw_hex(fb, regs[i], 16, (i+1) * 8);
}

void handle_fatal_error(int src, u32 *regs)
{
	while(1) {
		draw_regs(regs);
	}
}
